#include "Protocol.h"
#include "Robot.h"

static union
{
    float value;
    uint8_t TxData[4];
} txValue;

void Protocol_Callback(uint8_t *pData, uint8_t size)
{
    if (pData[0] == 0x1F && pData[7] == 0xF8)
    {
        for (uint8_t i = 0; i < 4; i++)
        {
            txValue.TxData[i] = pData[i + 3];
        }

        switch (pData[1])
        {
        case TSDA:
            if (pData[2] == SPEED)
            {
                Robot_SetSpeed(txValue.value);
            }
            break;

        case MG_LIFT:
            if (pData[2] == SPEED)
            {
            }
            else if (pData[2] == ANGLE)
            {
                Robot_SetLiftLength(txValue.value);
            }
            break;

        case MG_ROTATE:
            if (pData[2] == SPEED)
            {
            }
            else if (pData[2] == ANGLE)
            {
                Robot_SetRotateAngle(txValue.value);
            }
            break;

        case MG_CAMERA1:
            if (pData[2] == SPEED)
            {
            }
            else if (pData[2] == ANGLE)
            {
                Robot_SetCamera1Angle(txValue.value);
            }
            break;

        case MG_CAMERA2:
            if (pData[2] == SPEED)
            {
            }
            else if (pData[2] == ANGLE)
            {
                Robot_SetCamera2Angle(txValue.value);
            }
            break;

        case MG_CAMERA3:
            if (pData[2] == SPEED)
            {
            }
            else if (pData[2] == ANGLE)
            {
                Robot_SetCamera3Angle(txValue.value);
            }
            break;

        default:
            break;
        }
    }
}
